/*=========================================================================
 *
 *  Copyright UMC Utrecht and contributors
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#ifndef __itkMoreThuenteLineSearchOptimizer_cxx
#define __itkMoreThuenteLineSearchOptimizer_cxx

#include "itkMoreThuenteLineSearchOptimizer.h"
#include <cmath> // For abs.
#include <limits>

namespace itk
{

/**
 * ****************** Constructor *****************************
 */

MoreThuenteLineSearchOptimizer
::MoreThuenteLineSearchOptimizer()
{
  this->m_f                         = NumericTraits< MeasureType >::Zero;
  this->m_dg                        = 0.0;
  this->m_InitialDerivativeProvided = false;
  this->m_InitialValueProvided      = false;
  this->m_MaximumNumberOfIterations = 20;
  this->m_ValueTolerance            = 1e-4;
  this->m_GradientTolerance         = 0.9;
  this->m_IntervalTolerance         = std::numeric_limits< double >::epsilon();
  this->SetMinimumStepLength( 1e-20 );
  this->SetMaximumStepLength( 1e20 );

  this->InitializeLineSearch();

} // end Constructor


/**
 * *************** SetInitialValue *****************************
 */

void
MoreThuenteLineSearchOptimizer
::SetInitialValue( MeasureType value )
{
  this->m_InitialValueProvided = true;
  this->m_f                    = value;
  this->Modified();

} // end SetInitialValue()


/**
 * *************** SetInitialDerivative *****************************
 */

void
MoreThuenteLineSearchOptimizer
::SetInitialDerivative( const DerivativeType & derivative )
{
  this->m_InitialDerivativeProvided = true;
  this->m_g                         = derivative;
  this->Modified();

} // end SetInitialDerivative()


/**
 * *************** GetCurrentValueAndDerivative *********************
 */

void
MoreThuenteLineSearchOptimizer
::GetCurrentValueAndDerivative(
  MeasureType & value, DerivativeType & derivative ) const
{
  value      = m_f;
  derivative = m_g;

} // end GetCurrentValueAndDerivative()


/**
 * ************************ GetCurrentDerivative *********************
 */

void
MoreThuenteLineSearchOptimizer
::GetCurrentDerivative( DerivativeType & derivative ) const
{
  derivative = m_g;

} // end GetCurrentValueAndDerivative()


/**
 * ************************ GetCurrentValue *********************
 */

MoreThuenteLineSearchOptimizer::MeasureType
MoreThuenteLineSearchOptimizer
::GetCurrentValue( void ) const
{
  return m_f;

} // end GetCurrentValueAndDerivative()


/**
 * ************ GetCurrentDirectionalDerivative *********************
 */

double
MoreThuenteLineSearchOptimizer
::GetCurrentDirectionalDerivative( void ) const
{
  return m_dg;

} // end GetCurrentDirectionalDerivative()


/**
 * ************************** StartOptimization *********************
 */

void
MoreThuenteLineSearchOptimizer
::StartOptimization( void )
{
  this->CheckSettings();

  this->SetCurrentPosition( this->GetInitialPosition() );
  this->GetInitialValueAndDerivative();
  this->m_dg = this->DirectionalDerivative( this->m_g );

  this->InitializeLineSearch();

  this->InvokeEvent( StartEvent() );

  if(  this->m_dginit >= 0 )
  {
    this->m_StopCondition = AscentSearchDirection;
    this->StopOptimization();
  }

  while( !this->m_Stop )
  {

    this->UpdateIntervalMinimumAndMaximum();
    this->BoundStep( this->m_step );
    this->PrepareForUnusualTermination();
    this->SetCurrentStepLength( this->m_step );
    this->ComputeCurrentValueAndDerivative();
    this->m_dg = this->DirectionalDerivative( this->m_g );
    this->TestConvergence( this->m_Stop );
    this->InvokeEvent( IterationEvent() );
    if( this->m_Stop )
    {
      this->StopOptimization();
      break;
    }
    this->m_CurrentIteration++;
    this->ComputeNewStepAndInterval();
    this->ForceSufficientDecreaseInIntervalWidth();

  } // end while

} // end StartOptimization()


/**
 * ******************** StopOptimization******************************
 */

void
MoreThuenteLineSearchOptimizer
::StopOptimization( void )
{
  this->m_Stop = true;
  this->InvokeEvent( EndEvent() );

} // end StopOptimization()


/**
 * ******************** CheckSettings *********************************
 */

int
MoreThuenteLineSearchOptimizer
::CheckSettings( void )
{
  if( this->GetCostFunction() == 0 )
  {
    itkExceptionMacro( << "CostFunction has not been set!" );
  }

  const unsigned int numberOfParameters
    = this->GetCostFunction()->GetNumberOfParameters();

  if( this->GetInitialPosition().GetSize() != numberOfParameters )
  {
    itkExceptionMacro( << "InitialPosition has incorrect dimension!" );
  }

  if( this->GetLineSearchDirection().GetSize() != numberOfParameters )
  {
    itkExceptionMacro( << "LineSearchDirection has incorrect dimension!" );
  }

  if( this->GetMinimumStepLength() <= 0.0 )
  {
    itkExceptionMacro( << "MinimumStepLength must be higher than zero!" );
  }

  if( this->GetMinimumStepLength() > this->GetMaximumStepLength() )
  {
    itkExceptionMacro( << "MinimumStepLength must be smaller than MaximumStepLength!" );
  }

  if( this->GetGradientTolerance() < this->GetValueTolerance() )
  {
    itkExceptionMacro( << "GradientTolerance must be greater than ValueTolerance!" );
  }
  return 0;

}  // end CheckSettings()


/**
 * *************** GetInitialValueAndDerivative *********************
 */

void
MoreThuenteLineSearchOptimizer
::GetInitialValueAndDerivative( void )
{

  if( !( this->m_InitialValueProvided && this->m_InitialDerivativeProvided ) )
  {

    try
    {
      if( !this->m_InitialValueProvided && !this->m_InitialDerivativeProvided )
      {
        this->GetCostFunction()->GetValueAndDerivative(
          this->GetInitialPosition(),
          this->m_f,
          this->m_g );
      }
      else if( !this->m_InitialValueProvided )
      {
        this->m_f = this->GetCostFunction()->GetValue(
          this->GetInitialPosition() );
      }
      else if( !this->m_InitialDerivativeProvided )
      {
        this->GetCostFunction()->GetDerivative(
          this->GetInitialPosition(),
          this->m_g );
      }

    }
    catch( ExceptionObject & err )
    {
      this->m_StopCondition = MetricError;
      //this->StopOptimization(); //not here since no start event has been generated yet

      /** Any user provided initial values/derivatives may not be
       * valid anymore */
      this->m_InitialDerivativeProvided = false;
      this->m_InitialValueProvided      = false;

      throw err;
    }
  } // end if no initial value or derivative provided.

  /** Any user provided initial values/derivatives are not
   * valid anymore */

  this->m_InitialDerivativeProvided = false;
  this->m_InitialValueProvided      = false;

} // end GetInitialValueAndDerivative()


/**
 * ***************** InitializeLineSearch ******************************
 *
 * Set some member variables to their initial values.
 * Assumes m_f and m_dg have been set already.
 */

void
MoreThuenteLineSearchOptimizer
::InitializeLineSearch( void )
{
  this->m_Stop                                 = false;
  this->m_StopCondition                        = Unknown;
  this->m_CurrentIteration                     = 0;
  this->m_SufficientDecreaseConditionSatisfied = false;
  this->m_CurvatureConditionSatisfied          = false;
  this->m_CurrentStepLength                    = 0.0;

  this->m_finit                 = this->m_f;
  this->m_fx                    = this->m_finit;
  this->m_fy                    = this->m_finit;
  this->m_step                  = this->GetInitialStepLengthEstimate();
  this->m_stepx                 = 0.0;
  this->m_stepy                 = 0.0;
  this->m_stepmin               = 0.0;
  this->m_stepmax               = 0.0;
  this->m_dginit                = this->m_dg;
  this->m_dgx                   = this->m_dginit;
  this->m_dgy                   = this->m_dginit;
  this->m_dgtest                = this->GetValueTolerance() * this->m_dginit;
  this->m_width                 = this->GetMaximumStepLength() - this->GetMinimumStepLength();
  this->m_width1                = this->m_width / 0.5;
  this->m_brackt                = false;
  this->m_stage1                = true;
  this->m_SafeGuardedStepFailed = false;

} // end InitializeLineSearch()


/**
 * *************** UpdateIntervalMinimumAndMaximum ********************
 *
 * Set the minimum and maximum steps to correspond to the present
 * interval of uncertainty
 */

void
MoreThuenteLineSearchOptimizer
::UpdateIntervalMinimumAndMaximum( void )
{
  const double xtrapf = 4.0;

  if( this->m_brackt )
  {
    this->m_stepmin = std::min( this->m_stepx, this->m_stepy );
    this->m_stepmax = std::max( this->m_stepx, this->m_stepy );
  }
  else
  {
    this->m_stepmin = this->m_stepx;
    this->m_stepmax
      = this->m_step + xtrapf * ( this->m_step - this->m_stepx );
  }

} // end UpdateIntervalMinimumAndMaximum()


/**
 * ************************* BoundStep ********************************
 *
 * Force a step to be within the bounds MinimumStepLength and
 * MaximumStepLength
 */

void
MoreThuenteLineSearchOptimizer
::BoundStep( double & step ) const
{
  step = std::max( step, this->GetMinimumStepLength() );
  step = std::min( step, this->GetMaximumStepLength() );

} // end BoundStep()


/**
 * ******************** PrepareForUnusualTermination ********************
 *
 * If an unusual termination is to occur then let m_step be the lowest
 * point obtained so far
 */

void
MoreThuenteLineSearchOptimizer
::PrepareForUnusualTermination( void )
{
  if( ( this->m_brackt
    && ( this->m_step <= this->m_stepmin
    || this->m_step >= this->m_stepmax ) )
    || this->m_CurrentIteration >= this->GetMaximumNumberOfIterations() - 1
    || this->m_SafeGuardedStepFailed
    || ( this->m_brackt
    && this->m_stepmax - this->m_stepmin
    <= this->GetIntervalTolerance() * this->m_stepmax ) )
  {
    this->m_step = this->m_stepx;
  }

} // end PrepareForUnusualTermination()


/**
 * ***************** ComputeCurrentValueAndDerivative ********************
 *
 * Ask the cost function to compute m_f and m_g at the current position.
 */

void
MoreThuenteLineSearchOptimizer
::ComputeCurrentValueAndDerivative( void )
{
  try
  {
    this->GetCostFunction()->GetValueAndDerivative(
      this->GetCurrentPosition(), this->m_f, this->m_g );
  }
  catch( ExceptionObject & err )
  {
    this->m_StopCondition = MetricError;
    this->StopOptimization();
    throw err;
  }

} // end ComputeCurrentValueAndDerivative()


/**
 * ************************** TestConvergence ****************************
 *
 * Test for convergence.
 */

void
MoreThuenteLineSearchOptimizer
::TestConvergence( bool & stop )
{
  stop = false;
  const double & step = this->m_step;

  MeasureType ftest1 = this->m_finit + step * this->m_dgtest;

  this->m_SufficientDecreaseConditionSatisfied = ( this->m_f <= ftest1 );
  this->m_CurvatureConditionSatisfied          = ( std::abs( this->m_dg ) <=
    this->GetGradientTolerance() * ( -this->m_dginit ) );

  if( ( this->m_brackt
    && ( step <= this->m_stepmin || step >= this->m_stepmax ) )
    || this->m_SafeGuardedStepFailed )
  {
    this->m_StopCondition = RoundingError;
    stop                  = true;
  }

  if( step == this->GetMaximumStepLength()
    && this->m_SufficientDecreaseConditionSatisfied
    && this->m_dg <= this->m_dgtest )
  {
    this->m_StopCondition = StepTooLarge;
    stop                  = true;
  }

  if( step == this->GetMinimumStepLength()
    && ( !( this->m_SufficientDecreaseConditionSatisfied )
    || this->m_dg >= this->m_dgtest ) )
  {
    this->m_StopCondition = StepTooSmall;
    stop                  = true;
  }

  if( this->m_CurrentIteration >= this->GetMaximumNumberOfIterations() - 1 )
  {
    this->m_StopCondition = MaximumNumberOfIterations;
    stop                  = true;
  }

  if( this->m_brackt
    && this->m_stepmax - this->m_stepmin
    <= this->GetIntervalTolerance() * this->m_stepmax )
  {
    this->m_StopCondition = IntervalTooSmall;
    stop                  = true;
  }

  if( this->m_SufficientDecreaseConditionSatisfied
    && this->m_CurvatureConditionSatisfied  )
  {
    this->m_StopCondition = StrongWolfeConditionsSatisfied;
    stop                  = true;
  }

} // end TestConvergence()


/**
 * ****************** ComputeNewStepAndInterval ************************
 *
 * Update the interval of uncertainty and compute the new step
 */

void
MoreThuenteLineSearchOptimizer
::ComputeNewStepAndInterval( void )
{
  int returncode = 0;

  /** In the first stage we seek a step for which the modified
  * function has a nonpositive value and nonnegative derivative. */

  /** Stage1? */
  if( this->m_stage1
    && this->m_SufficientDecreaseConditionSatisfied
    && this->m_dg >= this->m_dginit * std::min(
    this->GetValueTolerance(), this->GetGradientTolerance() ) )
  {
    this->m_stage1 = false;
  }

  /* A modified function is used to predict the step only if
  * we have not obtained a step for which the modified
  * function has a nonpositive function value and nonnegative
  * derivative, and if a lower function value has been
  * obtained but the decrease is not sufficient. */

  if( this->m_stage1
    && this->m_f <= this->m_fx
    && !( this->m_SufficientDecreaseConditionSatisfied ) )
  {
    /* Define the modified function and derivative values. */
    const double & dgtest = this->m_dgtest;

    double fm   = this->m_f - this->m_step * dgtest;
    double fxm  = this->m_fx - this->m_stepx * dgtest;
    double fym  = this->m_fy - this->m_stepy * dgtest;
    double dgm  = this->m_dg - dgtest;
    double dgxm = this->m_dgx - dgtest;
    double dgym = this->m_dgy - dgtest;

    /* Call SafeGuardedStep to update the interval of uncertainty */
    /* and to compute the new step. */
    returncode = this->SafeGuardedStep(
      this->m_stepx, fxm, dgxm,
      this->m_stepy, fym, dgym,
      this->m_step, fm, dgm,
      this->m_brackt,
      this->m_stepmin, this->m_stepmax );

    /* Reset the function and gradient values. */
    this->m_fx  = fxm + this->m_stepx * dgtest;
    this->m_fy  = fym + this->m_stepy * dgtest;
    this->m_dgx = dgxm + dgtest;
    this->m_dgy = dgym + dgtest;
  }
  else
  {
    /* Call SafeGuardedStep to update the interval of uncertainty */
    /* and to compute the new step. */
    returncode = this->SafeGuardedStep(
      this->m_stepx, this->m_fx, this->m_dgx,
      this->m_stepy, this->m_fy, this->m_dgy,
      this->m_step, this->m_f, this->m_dg,
      this->m_brackt,
      this->m_stepmin, this->m_stepmax );
  }

  if( returncode == 0 )
  {
    this->m_SafeGuardedStepFailed = true;
  }

} //end ComputeNewStepAndInterval()


/**
 * ************** ForceSufficientDecreaseInIntervalWidth ******************
 *
 * Force a sufficient decrease in the size of the interval of uncertainty
 */

void
MoreThuenteLineSearchOptimizer
::ForceSufficientDecreaseInIntervalWidth( void )
{
  if( this->m_brackt )
  {
    const double & stx = this->m_stepx;
    const double & sty = this->m_stepy;

    if( std::abs( sty - stx ) >= .66 * this->m_width1 )
    {
      this->m_step = stx + .5 * ( sty - stx );
    }
    this->m_width1 = this->m_width;
    this->m_width  = std::abs( sty - stx );
  }

} // end ForceSufficientDecreaseInIntervalWidth()


/**
 * ************************** SafeGuardedStep ****************************
 *
 * Advance a step along the line search direction and update
 * the interval of uncertainty. Returns 0 if an error has occurred.
 */

int
MoreThuenteLineSearchOptimizer
::SafeGuardedStep(
  double & stx, double & fx, double & dx,
  double & sty, double & fy, double & dy,
  double & stp, const double & fp, const double & dp,
  bool & brackt,
  const double & stpmin, const double & stpmax ) const
{
  /** This function is largely just a copy of the following function
  * taken from netlib/lbfgs.c */

  /** Original documentation: */

  /**    void mcstep_(stx, fx, dx, sty, fy, dy, stp, fp, dp,
  *             brackt, stpmin, stpmax, info)
  *      doublereal *stx, *fx, *dx, *sty, *fy, *dy, *stp, *fp, *dp;
  *      logical *brackt;
  *      doublereal *stpmin, *stpmax;
  *      integer *info;
  */

  /*     SUBROUTINE MCSTEP */

  /*     THE PURPOSE OF MCSTEP IS TO COMPUTE A SAFEGUARDED STEP FOR */
  /*     A LINESEARCH AND TO UPDATE AN INTERVAL OF UNCERTAINTY FOR */
  /*     A MINIMIZER OF THE FUNCTION. */

  /*     THE PARAMETER STX CONTAINS THE STEP WITH THE LEAST FUNCTION */
  /*     VALUE. THE PARAMETER STP CONTAINS THE CURRENT STEP. IT IS */
  /*     ASSUMED THAT THE DERIVATIVE AT STX IS NEGATIVE IN THE */
  /*     DIRECTION OF THE STEP. IF BRACKT IS SET TRUE THEN A */
  /*     MINIMIZER HAS BEEN BRACKETED IN AN INTERVAL OF UNCERTAINTY */
  /*     WITH ENDPOINTS STX AND STY. */

  /*     THE SUBROUTINE STATEMENT IS */

  /*       SUBROUTINE MCSTEP(STX,FX,DX,STY,FY,DY,STP,FP,DP,BRACKT, */
  /*                        STPMIN,STPMAX,INFO) */

  /*     WHERE */

  /*       STX, FX, AND DX ARE VARIABLES WHICH SPECIFY THE STEP, */
  /*         THE FUNCTION, AND THE DERIVATIVE AT THE BEST STEP OBTAINED */
  /*         SO FAR. THE DERIVATIVE MUST BE NEGATIVE IN THE DIRECTION */
  /*         OF THE STEP, THAT IS, DX AND STP-STX MUST HAVE OPPOSITE */
  /*         SIGNS. ON OUTPUT THESE PARAMETERS ARE UPDATED APPROPRIATELY. */

  /*       STY, FY, AND DY ARE VARIABLES WHICH SPECIFY THE STEP, */
  /*         THE FUNCTION, AND THE DERIVATIVE AT THE OTHER ENDPOINT OF */
  /*         THE INTERVAL OF UNCERTAINTY. ON OUTPUT THESE PARAMETERS ARE */
  /*         UPDATED APPROPRIATELY. */

  /*       STP, FP, AND DP ARE VARIABLES WHICH SPECIFY THE STEP, */
  /*         THE FUNCTION, AND THE DERIVATIVE AT THE CURRENT STEP. */
  /*         IF BRACKT IS SET TRUE THEN ON INPUT STP MUST BE */
  /*         BETWEEN STX AND STY. ON OUTPUT STP IS SET TO THE NEW STEP. */

  /*       BRACKT IS A LOGICAL VARIABLE WHICH SPECIFIES IF A MINIMIZER */
  /*         HAS BEEN BRACKETED. IF THE MINIMIZER HAS NOT BEEN BRACKETED */
  /*         THEN ON INPUT BRACKT MUST BE SET FALSE. IF THE MINIMIZER */
  /*         IS BRACKETED THEN ON OUTPUT BRACKT IS SET TRUE. */

  /*       STPMIN AND STPMAX ARE INPUT VARIABLES WHICH SPECIFY LOWER */
  /*         AND UPPER BOUNDS FOR THE STEP. */

  /*       INFO IS AN INTEGER OUTPUT VARIABLE SET AS FOLLOWS: */
  /*         IF INFO = 1,2,3,4,5, THEN THE STEP HAS BEEN COMPUTED */
  /*         ACCORDING TO ONE OF THE FIVE CASES BELOW. OTHERWISE */
  /*         INFO = 0, AND THIS INDICATES IMPROPER INPUT PARAMETERS. */

  /*     SUBPROGRAMS CALLED */

  /*       FORTRAN-SUPPLIED ... ABS,MAX,MIN,SQRT */

  /*     ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. JUNE 1983 */
  /*     JORGE J. MORE', DAVID J. THUENTE */

  /** The info variable is now replaced by the returncode. */
  /** The variables are no longer passed as pointers, but by reference. */

  /* System generated locals */
  double d__1;

  /* Local variables */
  double sgnd, stpc, stpf, stpq, p, q, gamma, r, s, theta;
  bool   bound;

  int returncode = 0;

  /* CHECK THE INPUT PARAMETERS FOR ERRORS. */

  if( ( brackt && ( stp <= std::min( stx, sty )
    || stp >= std::max( stx, sty ) ) )
    || dx * ( stp - stx ) >= 0.
    || stpmax < stpmin )
  {
    return returncode;
  }

  /* DETERMINE IF THE DERIVATIVES HAVE OPPOSITE SIGN. */

  sgnd = dp * ( dx / std::abs( dx ) );

  /*     FIRST CASE. A HIGHER FUNCTION VALUE. */
  /*     THE MINIMUM IS BRACKETED. IF THE CUBIC STEP IS CLOSER */
  /*     TO STX THAN THE QUADRATIC STEP, THE CUBIC STEP IS TAKEN, */
  /*     ELSE THE AVERAGE OF THE CUBIC AND QUADRATIC STEPS IS TAKEN. */

  if( fp > fx )
  {
    returncode = 1;
    bound      = true;
    theta      = ( fx - fp ) * 3 / ( stp - stx ) + dx + dp;
    s          = std::max(
      std::max( std::abs( theta ), std::abs( dx ) ),
      std::abs( dp ) );
    d__1  = theta / s;
    gamma = s * std::sqrt( d__1 * d__1 - dx / s * ( dp / s ) );
    if( stp < stx )
    {
      gamma = -gamma;
    }
    p    = gamma - dx + theta;
    q    = gamma - dx + gamma + dp;
    r    = p / q;
    stpc = stx + r * ( stp - stx );
    stpq = stx
      + dx / ( ( fx - fp ) / ( stp - stx ) + dx ) / 2 * ( stp - stx );
    if( std::abs( stpc - stx ) < std::abs( stpq - stx ) )
    {
      stpf = stpc;
    }
    else
    {
      stpf = stpc + ( stpq - stpc ) / 2;
    }
    brackt = true;

    /*     SECOND CASE. A LOWER FUNCTION VALUE AND DERIVATIVES OF */
    /*     OPPOSITE SIGN. THE MINIMUM IS BRACKETED. IF THE CUBIC */
    /*     STEP IS CLOSER TO STX THAN THE QUADRATIC (SECANT) STEP, */
    /*     THE CUBIC STEP IS TAKEN, ELSE THE QUADRATIC STEP IS TAKEN. */

  }
  else if( sgnd < 0. )
  {
    returncode = 2;
    bound      = false;
    theta      = ( fx - fp ) * 3 / ( stp - stx ) + dx + dp;
    s          = std::max(
      std::max( std::abs( theta ), std::abs( dx ) ),
      std::abs( dp ) );
    d__1  = theta / s;
    gamma = s * std::sqrt( d__1 * d__1 - dx / s * ( dp / s ) );
    if( stp > stx )
    {
      gamma = -gamma;
    }
    p    = gamma - dp + theta;
    q    = gamma - dp + gamma + dx;
    r    = p / q;
    stpc = stp + r * ( stx - stp );
    stpq = stp + dp / ( dp - dx ) * ( stx - stp );
    if( std::abs( stpc - stp ) > std::abs( stpq - stp ) )
    {
      stpf = stpc;
    }
    else
    {
      stpf = stpq;
    }
    brackt = true;

    /*     THIRD CASE. A LOWER FUNCTION VALUE, DERIVATIVES OF THE */
    /*     SAME SIGN, AND THE MAGNITUDE OF THE DERIVATIVE DECREASES. */
    /*     THE CUBIC STEP IS ONLY USED IF THE CUBIC TENDS TO INFINITY */
    /*     IN THE DIRECTION OF THE STEP OR IF THE MINIMUM OF THE CUBIC */
    /*     IS BEYOND STP. OTHERWISE THE CUBIC STEP IS DEFINED TO BE */
    /*     EITHER STPMIN OR STPMAX. THE QUADRATIC (SECANT) STEP IS ALSO */
    /*     COMPUTED AND IF THE MINIMUM IS BRACKETED THEN THE THE STEP */
    /*     CLOSEST TO STX IS TAKEN, ELSE THE STEP FARTHEST AWAY IS TAKEN. */

  }
  else if( std::abs( dp ) < std::abs( dx ) )
  {
    returncode = 3;
    bound      = true;
    theta      = ( fx - fp ) * 3 / ( stp - stx ) + dx + dp;
    s          = std::max(
      std::max( std::abs( theta ), std::abs( dx ) ),
      std::abs( dp ) );

    /* THE CASE GAMMA = 0 ONLY ARISES IF THE CUBIC DOES NOT TEND */
    /* TO INFINITY IN THE DIRECTION OF THE STEP. */

    d__1  = theta / s;
    d__1  = d__1 * d__1 - dx / s * ( dp / s );
    gamma = s * std::sqrt( std::max( 0., d__1 ) );
    if( stp > stx )
    {
      gamma = -gamma;
    }
    p = gamma - dp + theta;
    q = gamma + ( dx - dp ) + gamma;
    r = p / q;
    if( r < 0. && gamma != 0. )
    {
      stpc = stp + r * ( stx - stp );
    }
    else if( stp > stx )
    {
      stpc = stpmax;
    }
    else
    {
      stpc = stpmin;
    }
    stpq = stp + dp / ( dp - dx ) * ( stx - stp );
    if( brackt )
    {
      if( std::abs( stp - stpc ) < std::abs( stp - stpq ) )
      {
        stpf = stpc;
      }
      else
      {
        stpf = stpq;
      }
    }
    else
    {
      if( std::abs( stp - stpc ) > std::abs( stp - stpq ) )
      {
        stpf = stpc;
      }
      else
      {
        stpf = stpq;
      }
    }

    /*     FOURTH CASE. A LOWER FUNCTION VALUE, DERIVATIVES OF THE */
    /*     SAME SIGN, AND THE MAGNITUDE OF THE DERIVATIVE DOES */
    /*     NOT DECREASE. IF THE MINIMUM IS NOT BRACKETED, THE STEP */
    /*     IS EITHER STPMIN OR STPMAX, ELSE THE CUBIC STEP IS TAKEN. */

  }
  else
  {
    returncode = 4;
    bound      = false;
    if( brackt )
    {
      theta = ( fp - fy ) * 3 / ( sty - stp ) + dy + dp;
      s     = std::max(
        std::max( std::abs( theta ), std::abs( dy ) ),
        std::abs( dp ) );
      d__1  = theta / s;
      gamma = s * std::sqrt( d__1 * d__1 - dy / s * ( dp / s ) );
      if( stp > sty )
      {
        gamma = -gamma;
      }
      p    = gamma - dp + theta;
      q    = gamma - dp + gamma + dy;
      r    = p / q;
      stpc = stp + r * ( sty - stp );
      stpf = stpc;
    }
    else if( stp > stx )
    {
      stpf = stpmax;
    }
    else
    {
      stpf = stpmin;
    }
  }

  /*     UPDATE THE INTERVAL OF UNCERTAINTY. THIS UPDATE DOES NOT */
  /*     DEPEND ON THE NEW STEP OR THE CASE ANALYSIS ABOVE. */

  if( fp > fx )
  {
    sty = stp;
    fy  = fp;
    dy  = dp;
  }
  else
  {
    if( sgnd < 0. )
    {
      sty = stx;
      fy  = fx;
      dy  = dx;
    }
    stx = stp;
    fx  = fp;
    dx  = dp;
  }

  /*     COMPUTE THE NEW STEP AND SAFEGUARD IT. */

  stpf = std::min( stpmax, stpf );
  stpf = std::max( stpmin, stpf );
  stp  = stpf;
  if( brackt && bound )
  {
    if( sty > stx )
    {
      d__1 = stx + ( sty - stx ) * .66f;
      stp  = std::min( d__1, stp );
    }
    else
    {
      d__1 = stx + ( sty - stx ) * .66f;
      stp  = std::max( d__1, stp );
    }
  }

  return returncode;

} // end SafeGuardedStep()


/**
 * ****************** PrintSelf *****************************
 */

void
MoreThuenteLineSearchOptimizer
::PrintSelf( std::ostream & os, Indent indent ) const
{
  /** Call the superclass' PrintSelf. */
  Superclass::PrintSelf( os, indent );

  os << indent << "m_CurrentIteration: "
     << this->m_CurrentIteration << std::endl;
  os << indent << "m_InitialDerivativeProvided: "
     << ( this->m_InitialDerivativeProvided ? "true" : "false" ) << std::endl;
  os << indent << "m_InitialValueProvided: "
     << ( this->m_InitialValueProvided ? "true" : "false" ) << std::endl;
  os << indent << "m_StopCondition: "
     << this->m_StopCondition << std::endl;
  os << indent << "m_Stop: "
     << ( this->m_Stop ? "true" : "false" ) << std::endl;
  os << indent << "m_SufficientDecreaseConditionSatisfied: "
     << ( this->m_SufficientDecreaseConditionSatisfied ? "true" : "false" ) << std::endl;
  os << indent << "m_CurvatureConditionSatisfied: "
     << ( this->m_CurvatureConditionSatisfied ? "true" : "false" ) << std::endl;

  os << indent << "m_step: "
     << this->m_step << std::endl;
  os << indent << "m_stepx: "
     << this->m_stepx << std::endl;
  os << indent << "m_stepy: "
     << this->m_stepy << std::endl;
  os << indent << "m_stepmin: "
     << this->m_stepmin << std::endl;
  os << indent << "m_stepmax: "
     << this->m_stepmax << std::endl;

  os << indent << "m_f: "
     << this->m_f << std::endl;
  os << indent << "m_fx: "
     << this->m_fx << std::endl;
  os << indent << "m_fy: "
     << this->m_fy << std::endl;
  os << indent << "m_finit: "
     << this->m_finit << std::endl;

  os << indent << "m_g: "
     << this->m_g << std::endl;
  os << indent << "m_dg: "
     << this->m_dg << std::endl;
  os << indent << "m_dginit: "
     << this->m_dginit << std::endl;
  os << indent << "m_dgx: "
     << this->m_dgx << std::endl;
  os << indent << "m_dgy: "
     << this->m_dgy << std::endl;
  os << indent << "m_dgtest: "
     << this->m_dgtest << std::endl;

  os << indent << "m_width: "
     << this->m_width << std::endl;
  os << indent << "m_width1: "
     << this->m_width1 << std::endl;
  os << indent << "m_brackt: "
     << ( this->m_brackt ? "true" : "false" ) << std::endl;
  os << indent << "m_stage1: "
     << ( this->m_stage1 ? "true" : "false" ) << std::endl;
  os << indent << "m_SafeGuardedStepFailed: "
     << ( this->m_SafeGuardedStepFailed ? "true" : "false" ) << std::endl;

  os << indent << "m_MaximumNumberOfIterations: "
     << this->m_MaximumNumberOfIterations << std::endl;
  os << indent << "m_ValueTolerance: "
     << this->m_ValueTolerance << std::endl;
  os << indent << "m_GradientTolerance: "
     << this->m_GradientTolerance << std::endl;
  os << indent << "m_IntervalTolerance: "
     << this->m_IntervalTolerance << std::endl;

} // end PrintSelf()


} // end namespace itk

#endif // #ifndef __itkMoreThuenteLineSearchOptimizer_cxx
